#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl\io\pcd_io.h>
//#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>

int main (int argc, char** argv) {
	for (int i=1; i<argc; i++) {
	  //Open file from argv[1]
	  pcl::PCDReader reader;
      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudx (new pcl::PointCloud<pcl::PointXYZRGBA>);
      reader.read (argv[i], *cloudx);
      pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
      // Create the VFH estimation class, and pass the input dataset+normals to it
      pcl::VFHEstimation<pcl::PointXYZRGBA, pcl::Normal, pcl::VFHSignature308> vfh;
      vfh.setInputCloud (cloudx);
      //normals
      // Create the normal estimation class, and pass the input dataset to it
	  pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
      ne.setInputCloud (cloudx);

      // Create an empty kdtree representation, and pass it to the normal estimation object.
      // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
      pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree0 (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
      ne.setSearchMethod (tree0);

      // Output datasets
      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

      // Use all neighbors in a sphere of radius 3cm
      ne.setRadiusSearch (0.03);

      // Compute the features
      ne.compute (*cloud_normals);
      vfh.setInputNormals (cloud_normals);
      // alternatively, if cloud is of type PointNormal, do vfh.setInputNormals (cloud);

      // Create an empty kdtree representation, and pass it to the FPFH estimation object.
      // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
      pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
      vfh.setSearchMethod (tree);

      // Output datasets
      pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());

      // Compute the features
      vfh.compute (*vfhs);
	  std::stringstream ss;
      ss << argv[i] << "_vfh.pcd";
      // vfhs->points.size () should be of size 1*
	  pcl::io::savePCDFileASCII (ss.str(), *vfhs);
	}
  return 0;
 }